/**
 * @file Bone.cpp
 * @author  Aljosa Osep <aljosa.osep@gmail.com>
 * @version 1.0
 *
 * @section LICENSE
 *
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:

 *  1. Redistributions of source code must retain the above copyright notice, this list of
 *    conditions and the following disclaimer.

 *  2. Redistributions in binary form must reproduce the above copyright notice, this list
 *     of conditions and the following disclaimer in the documentation and/or other materials
 *     provided with the distribution.

 * THIS SOFTWARE IS PROVIDED BY <Aljosa Osep> ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL <Aljosa Osep>> OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 * The views and conclusions contained in the software and documentation are those of the
 * authors and should not be interpreted as representing official policies, either expressed
 * or implied, of <Aljosa Osep>.
 *
 * @section DESCRIPTION
 *
 * Bone - Implementation of Bone2 and Bone3 classes
 */

#include "bone.h"
#include "MathUtil.h"
#include <iostream>

#include <GL/gl.h>  // TODO: remove
#include <GL/glu.h>

using namespace std;

/// Constructors
/**
 * Bone Constructor
 *
 * @param  unique id (short)
 * @param orient (Body part orientation: LEFT, RIGHT or NONE)
 */
Bone2::Bone2(short id, const Orientation &orient) : id(id), numChildren(0), orientation(orient) {  // Default constructor
    // Set childrens to NULL
    for (int i=0; i<MAX_CHILDREN; i++)
        this->children[i] = NULL;

    cout<<"Bone created, id: "<<id<<endl;
}

/**
 * Bone Constructor
 *
 * @param  unique id (short)
 * @param pos (Vector3, relative position)
 * @param angle
 * @param orient (Body part orientation: LEFT, RIGHT or NONE)
 */
Bone2::Bone2(short id, Math::Vector3 pos, float length, float angle, const Orientation &orient) : id(id), pos(pos), length(length), angle(angle), numChildren(0), orientation(orient) {
    // Set childrens to NULL
     for (int i=0; i<MAX_CHILDREN; i++)
        this->children[i] = NULL;

    cout<<"Bone created, id: "<<id<<" length: "<<length<<" angle: "<<angle<<endl;
}

/**
 * Bone destructor
 *
 * Delete's bone and it's children
 *
 */
Bone2::~Bone2() {
    this->release();
}

/// Methods
/**
 * Adds child bone
 *
 * Attaches given bone to a root bone
 *
 * @param  Bone to add(Bone2)
 * @return True or false wheter bone was successfully added or not
 */
bool Bone2::addChildBone(Bone2 *bone) { // Add children bone method
    // Do we have a bone?
    if (bone == NULL) {
        cout<<"Error: no bone"<<endl;
        return false;
    }

    // Check if we can add another child bone
    if (this->numChildren >= MAX_CHILDREN) {
        cout<<"Can not add bone to id: "<<id<<", children overflow."<<endl;
        return false;
    }

    // We can add bone
    this->children[numChildren++] = bone;

    return true;

}

/**
 * Release
 *
 * Releases the bone
 *
 */
void Bone2::release() {
    // Check if we have children
    if (this->children > 0) {
        // We do
        for (int i=0; i<this->numChildren; i++) {
            this->children[i]->release(); // Release child
            delete this->children[i]; // Delete it
            this->children[i] = NULL; // And set pointer to NULL


        }

        this->numChildren = 0;
        cout<<"Bone with id "<<id<<" released."<<endl;
    }
}

/**
 * Print bone
 *
 * Writes bone informations into the console
 *
 */
void Bone2::print() const {
    // Print id's
    cout<<this->id<<' ';

    // Print children
    if (this->numChildren > 0) {
        cout<<endl;

        for (int i=0; i<this->numChildren; i++) {
            this->children[i]->print();
        }
    }
}

/**
 * Sets the bone angle limits
 *
 */
void Bone2::setLimits(float max, float min) {
    this->minAngle = min;
    this->maxAngle = max;
}

void Bone2::draw() {
    // WARNING: UNFLEKSIBLE CODE HERE, FIX
    glPushMatrix();

    glTranslatef(this->pos.x, this->pos.y, this->pos.z);
    glRotatef(Rad2Deg(this->getAngle()), 0.0f, 0.0f, 1.0f);

    glColor3f(1.0f, 0.0f, 0.0f); // TEMPORARY

    // Not very nice, todo: fix
    glBegin(GL_LINES);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(this->length, 0.0f, 0.0f);
    glEnd();

    glTranslatef(this->length, 0.0f, 0.0f);

    // draw the children
    for (int i=0; i<this->numChildren; i++)
        this->children[i]->draw();

    glPopMatrix();

}

/// Setters, getters
/**
 * Sets the bone angle
 *
 */
void Bone2::setAngle(const float theta) {
   /* if ( orientation == LEFT ) // Angle is in quadrant II
        this->angle = kPi - theta;
    else if ( orientation == RIGHT || orientation == NONE )
        this->angle = theta;*/
    this->angle = theta; // Set the angle
}

/**
 * Returns the bone angle
 *
 */
float Bone2::getAngle() const {
     if ( orientation == LEFT ) // Angle is in quadrant II
        return kPi - this->angle; // Therefore an actual angle is PI - angle
    else //if ( orientation == RIGHT ) // || orientation == NONE )
        return this->angle;
}




